// PID.java
// Jeremy Singer
// 31 Aug 11

package org.mmtk.utility.heap;

public abstract class PID {

  public double kp;  // proportional gain
  public double ki;   // integral gain
  public double kd;  // derivative gain

  public double target;  // target value of output variable
  public double input;      // current value of input variable

  public double integral;  // I value for PID controller
  public double prevError;  // previous error value from last iteration
  
  public PID(double kp, double ki, double kd, double target) {
    // init params
    this.kp = kp;
    this.ki = ki;
    this.kd = kd;
    this.target = target;

    // init controller variables
    this.input = 0;
    this.integral = 0;
    this.prevError = 0;
    
  }

  // accessor methods
  public double getInput() {
    return this.input;
  }

  /** 
   * compound setting of all three K parameters for PID equation
   */
  public void setPIDParams(double kp, double ki, double kd) {
    setKp(kp);
    setKi(ki);
    setKd(kd);
  }

  public void setKp(double kp) {
    this.kp = kp;
  }

  public void setKi(double ki) {
    this.ki = ki;
  }

  public void setKd(double kd) {
    this.kd = kd;
  }

  public void setTarget(double target) {
    this.target = target;
  }
  
  public double getKp() {
    return this.kp;
  }

  public double getKi() {
    return this.ki;
  }

  public double getKd() {
    return this.kd;
  }

  public double getTarget() {
    return this.target;
  }

  /**
   * Execute a single iteration of the PID controller calculation
   * @arg dt the time delta since the previous iteration
   * @sideeffect updates input value based on PID equation
   */
  public void runOneIter(long delta) {
	double dt = delta / 2^20;
    double currVal = readCurrentOutputVal();
    double error = currVal - target;
    this.integral = integral + (error*dt);
    double derivative = (error-prevError) / dt;
    double newInputValue = (kp*error) + (ki*this.integral) * (kd*derivative);
    // Now update input value
    this.input = newInputValue;
  }

  /**
   * @return the current value of the output (observed) variable
   */
  public abstract double readCurrentOutputVal();


    /**
     * outputs a string debugging representation of PID
     */
    public String toString() {
	String s;
	s = "kp = " + this.kp +", ki = " + this.ki +
	    ", kd = " + this.kd + ", target = " + target + ".";
	return s;
    }

}